;********************************************************************************************************
;                                      OS_TASK.s
;********************************************************************************************************

    IMPORT  gp_task_cur
	IMPORT  gp_task_rdy
	IMPORT  gp_stack_schedule
	IMPORT  os_taskc_sched

    EXPORT 	OS_TASK_BLOCK
	EXPORT  OS_TASK_RESUME

    AREA |.text|, CODE, READONLY, ALIGN=2
    THUMB
    REQUIRE8
    PRESERVE8
	
OS_TASK_BLOCK 
	;STMFD	sp!, {r0-r12, lr}	; save register file and ret address for cortex-m3 m4 m7
	PUSH 	{r0-r3, lr}        	; save register file and ret address for cortex-m0
	
	MRS     r1, MSP              ; get from thread stack pointer    
	SUBS    r1, r1, #0x28       ; space for {PSR, r4 - r12} 
	MSR		MSP, r1
		
	LDR		r0, =gp_task_cur	; Get current task TCB address
	LDR     r2, [r0]    		; store sp in preempted tasks TCB
	STR     r1, [r2]            ; update from thread stack pointer 

	MRS		r3, PSR
	STMIA   r1!, {r3 - r7}      ; push thread {PSR, r4 - r7} register to thread stack    
	
	MOV		r3, r8
	MOV     r4, r9              ; mov thread {r8 - r12} to {r3 - r7}    
	MOV     r5, r10    
	MOV     r6, r11    
	MOV     r7, r12    
	STMIA   r1!, {r3 - r7}      ; push thread {r8 - r13} high register to thread stack
	
	LDR 	r0, =gp_stack_schedule	; Go to task schedule
	LDR 	r2, [r0]
	MSR 	MSP, r2
	LDR 	r0, =os_taskc_sched
	BX 		r0
	

OS_TASK_RESUME
	LDR		r2, =gp_task_rdy	; Get highest priority task TCB address
	LDR		r2, [r2]
	LDR		r1, [r2]			; get new task stack pointer	
	MSR		MSP, r1
	
	LDR		r3, =gp_task_cur	; OSTCBCur = OSTCBHighRdy
	STR		r2, [r3]			; set new current task TCB address

	;LDMFD	sp!, {r4}			; YYY+
	;MSR	PSR, r4				; YYY+
	;LDMFD	sp!, {r0-r12, pc}	; YYY+
  
	LDMIA   r1!, {r3 - r7}      ; pop thread {PSR, r4 - r7} register from thread stack    
	PUSH    {r3 - r7}           ; push {r3 - r7} to SP for copy {r8 - r12}    
	LDMIA   r1!, {r3 - r7}      ; pop thread {r8 - r12} high register from thread stack to {r4 - r7}    
	MOV     r8,  r3             ; mov {r4 - r7} to {r8 - r11}    
	MOV     r9,  r4    
	MOV     r10, r5    
	MOV     r11, r6
	MOV     r12, r7
	POP     {r3 - r7}           ; pop {PSR, r4 - r7} from SP
	MSR		PSR, r3
	MSR     MSP, r1              ; update stack pointer

	POP		{r0-r3, pc}		
		
	ALIGN		
	END		
	
